/*
 * VmMap.cpp
 *
 *  Created on: 10.04.2010
 *      Author: artur
 */

#include <kiddie/kiddie.h>
#include <platform.h>
#include <sys/errno.h>

#include <Console.h>
#include <hw/Platform.h>
#include <mem/MemTypes.h>
#include <mem/VmMap.h>

extern kernel::Console cout;
extern kernel::Platform hwplatform;

namespace kernel
{
  namespace mem
  {
    /**
     *
     */
    int
    VmMap::_normalize(MemArea *area)
    {
      MemArea *_area;
      vaddr_t addr_end;
      int ret;
      ENTER;
      OUTDEBUG("[ VmMap ]");
      ret = 0;
      for (_area = this->usedareas.next; _area && (_area != &this->usedareas); _area
          = _area->next)
        {
          if (area == _area)
            {
              continue;
            }
          if (area->v_addr >= _area->v_addr && area->v_addr <= (_area->v_addr
              + _area->length))
            {
              addr_end = ((area->v_addr + area->length) > (_area->v_addr
                  + _area->length)) ? (area->v_addr + area->length)
                  : (_area->v_addr + _area->length);
              _area->length = addr_end - _area->v_addr;
              ret = 1;
              break;
            }
        }

      EXIT;
      return ret;
    }

    /**
     *
     */
    int
    VmMap::normalize(void)
    {
      int run;
      MemArea *area, *tmp_area;
      ENTER;
      OUTDEBUG("[ VmMap ]");
      for (area = this->usedareas.next; area && area != &this->usedareas;)
        {
          tmp_area = NULL;
          if (_normalize(area))
            {
              /* remove area from list */
              area->prev->next = area->next;
              area->next->prev = area->prev;
              tmp_area = area;
            }
          area = area->next;
          // if (NULL != tmp_area) kfree(tmp_area);
        }
      EXIT;
      return 0;
    }

    /**
     *
     */
    vaddr_t
    VmMap::getFreeVAddr(paddr_t paddr, size_t siz)
    {
      MemArea *_area, *tmp_area;
      size_t len, off = paddr % PAGE_SIZE;
      vaddr_t vaddr;
      OUTDEBUG("[ VmMap ]");
      len = PAGE_ALIGN(off + siz);

      _area = &this->freeareas;
      do
        {
          if (len < _area->length)
            {
              vaddr = (vaddr_t) PAGE_TRUNC(_area->v_addr);
              _area->v_addr += len;
              _area->length -= len;
              _area->length = PAGE_TRUNC(_area->length);
              return vaddr;
            }
          _area = _area->next;
        }
      while (_area != &this->freeareas);

      ::cout << "getFree " << (int) ((len)) << "," << (int) (vaddr) << "\n";
      return NULL;
    }

    /**
     *
     */
    vaddr_t
    VmMap::remap(paddr_t paddr, vaddr_t _vaddr, size_t siz)
    {
      size_t off = paddr % PAGE_SIZE;
      int num = PAGE_ALIGN(off + siz) / PAGE_SIZE;

      // OUTDEBUG("[ VmMap ]");
      cout << kernel::Console::HEX << "[ VmMap ] remapping " << num
          << " pages from paddr(0x" << (int) (PAGE_TRUNC(paddr))
          << ")  to vaddr(0x" << (int) (PAGE_TRUNC(_vaddr)) << ")"
          << kernel::Console::DEC << (int) (PAGE_ALIGN(off + siz)) << "]\n";

      doMapping(kernel::mem::MemArea(PAGE_TRUNC(paddr), (PAGE_TRUNC(_vaddr)),
          PAGE_ALIGN(off + siz), false));
      return _vaddr + off;
    }

    /**
     *
     */
    vaddr_t
    VmMap::remap(paddr_t paddr, size_t siz)
    {
      //OUTDEBUG("[ VmMap ]");
      vaddr_t vaddr = this->getFreeVAddr(paddr, siz);
      cout << kernel::Console::HEX
          << "[ VmMap ] VmMap::remap() remapping paddr(0x" << (int) (paddr)
          << ") vaddr(0x" << (int) (vaddr) << ") SIZ[" << kernel::Console::DEC
          << (int) (siz) << "]\n";
      vaddr = remap(paddr, vaddr, siz);
      return vaddr;
    }

    /**
     *
     */
    vaddr_t
    VmMap::doMapping(void)
    {
      MemArea *area;
      for (area = this->usedareas.next; area && (area != &this->usedareas); area
          = area->next)
        {
          if (area->length > 0)
            this->doMapping(*area);
        }
    }

    /**
     * Map one page
     */
    vaddr_t
    VmMap::mapPage(paddr_t paddr, vaddr_t vaddr)
    {
      int _i; /* starting page table index */
      int _j; /* starting page directory index */
      pgd_t vpgd; /* pointer to virtual address of page directory */
      pgd_t pgt1, vpgt1; /* ptr to physical and virtual addresses of page table */

      vpgd = (pgd_t) phys_to_virt(this->pgd);

      /* PGT index */
      _j = pgd_idx(vaddr);

      /* Initial page index */
      _i = pgt_idx(vaddr);

      /* Physical page address (if used by remap() syscall) */
      paddr = PAGE_TRUNC(paddr);

      /* take 'pgt' */
      if ((vpgd[_j] & PDE_PRESENT))
        {
          pgt1 = (pgd_t)(vpgd[_j] & ~(PAGE_SIZE - 1));
        }
      // Page table is not present
      else
        {
          pgt1 = (pgd_t) ::hwplatform.pageFactory.alloc(PAGE_SIZE, 0);
        }

      /* get virtual address of pgt */
      vpgt1 = (pgd_t) phys_to_virt(pgt1);

      /* if page is present do nothing */
      if ((vpgt1[_i] & PTE_PRESENT))
        {
          paddr = vpgt1[_i] & ~(PAGE_SIZE - 1);
          return paddr;
        }
      /* If page is not present */
      else
        {
          // paddr = (paddr_t) ::hwplatform.pageFactory.alloc(PAGE_SIZE,0);
          vpgt1[_i] = paddr | PTE_PRESENT | PTE_WRITE | PTE_USER;
        }

      /* map page table */
      vpgd[_j] = (u32)((pgt1)) | PDE_PRESENT | PDE_WRITE | PDE_USER;

      return paddr;
    }

    /**
     * Map one page
     */
    vaddr_t
    VmMap::mapPage(vaddr_t vaddr)
    {
      int _i; /* starting page table index */
      int _j; /* starting page directory index */
      paddr_t paddr;
      pgd_t vpgd; /* pointer to virtual address of page directory */
      pgd_t pgt1, vpgt1; /* ptr to physical and virtual addresses of page table */

      vpgd = (pgd_t) phys_to_virt(this->pgd);

      /* PGT index */
      _j = pgd_idx(vaddr);

      /* Initial page index */
      _i = pgt_idx(vaddr);

      /* take 'pgt' */
      if ((vpgd[_j] & PDE_PRESENT))
        {
          pgt1 = (pgd_t)(vpgd[_j] & ~(PAGE_SIZE - 1));
        }
      // Page table is not present
      else
        {
          pgt1 = (pgd_t) ::hwplatform.pageFactory.alloc(PAGE_SIZE, 0);
          /* map page table */
          vpgd[_j] = (u32)((pgt1)) | PDE_PRESENT | PDE_WRITE | PDE_USER;
        }

      /* get virtual address of pgt */
      vpgt1 = (pgd_t) phys_to_virt(pgt1);

      /* if page is present do nothing */
      if (!(vpgt1[_i] & PTE_PRESENT))
        {
          paddr = (paddr_t) ::hwplatform.pageFactory.alloc(PAGE_SIZE, 0);
          vpgt1[_i] = paddr | PTE_PRESENT | PTE_WRITE | PTE_USER;
        }

      return paddr;
    }

    /**
     * TODO This (mapping) should be done somewhere in a page or
     * platform context.
     */
    vaddr_t
    VmMap::doMapping(const kernel::mem::MemArea & area)
    {
      int _i; /* starting page table index */
      int i_; /* ending page table index */
      int _j; /* starting page directory index */
      int j_; /* ending page directory index */
      int num; /* number of pages in area */
      paddr_t address; /* physical address variable */
      vaddr_t _end; /* last virtual address of memory area */
      pgd_t vpgd; /* pointer to virtual address of page directory */
      pgd_t pgt1, vpgt1; /* ptr to physical and virtual addresses of page table */

      vpgd = (pgd_t) phys_to_virt(this->pgd);
      _end = area.v_addr + area.length;

      /* PGT index */
      _j = pgd_idx(area.v_addr);
      j_ = pgd_idx(_end);

      /* Initial page index */
      _i = pgt_idx(area.v_addr);
      /* Number of pages to map */
      num = (int) (PAGE_ALIGN((area.length + (area.v_addr % PAGE_SIZE)))
          / PAGE_SIZE);

#if 0
#ifdef DEBUG
      ::cout << "[ VmMap ] " << __FUNCTION__ << kernel::Console::HEX << "() Vaddr=0x"
      << (int) area.v_addr << ": "
      << kernel::Console::DEC << " _j=" << _j << ",j_=" << j_ << ",i=" << _i << ",numpages=" << num
      << "; len=" << (int)area.length << "\n";
#endif
#endif

      /* Physical page address (if used by remap() syscall) */
      address = PAGE_TRUNC(area.p_addr);

      /* main mapping loop */
      for (_j; (_j <= j_) && num; _j++)
        {

          /* take 'pgt' */
          if ((vpgd[_j] & PDE_PRESENT))
            {
              pgt1 = (pgd_t)(vpgd[_j] & ~(PAGE_SIZE - 1));
            }
          // Page table is not present
          else
            {
              pgt1 = (pgd_t) ::hwplatform.pageFactory.alloc(PAGE_SIZE, 0);
            }

          /* get virtual address of pgt */
          vpgt1 = (pgd_t) phys_to_virt(pgt1);

          /* do page mapping */
          while (num--)
            {
              /* if page is present do nothing */
              if ((vpgt1[_i] & PTE_PRESENT))
                {
                  address = vpgt1[_i] & ~(PAGE_SIZE - 1);
                }
              /* If page is not present */
              else
                {
                  if (area.alloc)
                    {
                      address = (paddr_t) ::hwplatform.pageFactory.alloc(
                          PAGE_SIZE, 0);
                      vpgt1[_i] = address | PTE_PRESENT | PTE_WRITE | PTE_USER;
                    }
                  else
                    {
                      vpgt1[_i] = address | PTE_PRESENT | PTE_WRITE | PTE_USER;
                      address += PAGE_SIZE;
                    }
                }

              _i++;
              if (_i >= pgt_len())
                {
                  _i = 0;
                  /* exit 'while(num--)' */
                  break;
                }
            }

#ifdef DEBUG
          ::cout << "[ VmMap ] " << __FUNCTION__
          << kernel::Console::DEC << "() ###---->>> &&& Mapping PGD: _j=" << _j << ",j_=" << j_ << ",i=" << _i
          << "; len=" << (int)area.length << "\n";
#endif
          /* map page table */
          vpgd[_j] = (u32)((pgt1)) | PDE_PRESENT | PDE_WRITE | PDE_USER;

        }
    }

    /**
     *
     */
    VmMap &
    VmMap::operator +=(const MemArea & blk)
    {
      MemArea *area;
      vaddr_t addr_start, addr_end;
      ENTER;
      if (NULL == blk.v_addr || blk.length <= 0)
        {
          ::cout << "[MemMap]  ERROR!! Invalid parameters\n";
          EXIT;
          goto plusout;
        }
      addr_start = PAGE_TRUNC(blk.v_addr);
      addr_end = PAGE_ALIGN(blk.v_addr + blk.length);
      if (addr_end >= (vaddr_t) & KERNEL_BASE)
        {
          ::cout << "[vm] ERROR!! Invalid mem area\n";
          EXIT;
          goto plusout;
        }
      for (area = this->usedareas.next; area && (area != &this->usedareas); area
          = area->next)
        {
          // OUTDEBUG("[ VmMap ]");
          /* find overlapped area */
          if (addr_start >= area->v_addr && addr_start <= (area->v_addr
              + area->length))
            {
              // OUTDEBUG("[ VmMap ]");
              /* create union  */
              addr_end = (addr_end > (area->v_addr + area->length)) ? addr_end
                  : (area->v_addr + area->length);
              area->length = addr_end - area->v_addr;
              ::cout << kernel::Console::HEX << "[vm] New area: "
                "area->v_addr=0x" << (int) area->v_addr << ", area->length=0x"
                  << (int) area->length << "\n" << kernel::Console::HEX;
              EXIT;
              goto plusout;
            }
        }
      area = new MemArea();
      if (!area)
        {
          ::cout << "[vm] ERROR! No mem space\n";
          EXIT;
          goto plusout;
        }
      area->length = addr_end - addr_start;
      area->v_addr = addr_start;
      area->prev = this->usedareas.prev;
      area->next = &this->usedareas;
      area->prev->next = area;
      area->alloc = blk.alloc;
      this->usedareas.prev = area;
      EXIT;
      plusout: return *this;
    }

    /**
     *
     */
    VmMap &
    VmMap::operator -=(const MemArea & blk)
    {
      return *this;
    }

    /**
     *
     */
    VmMap &
    VmMap::operator =(VmMap & _from)
    {
      pgd_t _pgd1;
      pgd_t _pgd2;
      ENTER;
      OUTDEBUG("[ VmMap ]");
      if (this->pgd && _from.pgd)
        {
          OUTDEBUG("[ VmMap ]");
          // get virtual address of new page directory
          _pgd1 = (pgd_t) phys_to_virt(this->pgd);
          _pgd2 = (pgd_t) phys_to_virt(_from.pgd);
          // copy kernel pgd to new pgd
          util::memcpy(_pgd1, (unsigned char*) _pgd2, PAGE_SIZE);
        }
      EXIT;

      _init();

      OUTDEBUG("[ VmMap ]");
      return *this;
    }

    /**
     *
     */
    VmMap::VmMap()
    {
      OUTDEBUG("[ VmMap ] Constructor ");
      pgd = 0;
      _init();
    }

    /**
     *
     */
    VmMap::VmMap(pgd_t p) :
      pgd(p)
    {
      OUTDEBUG("[ VmMap ] Constructor ");
      _init();
    }

    /**
     *
     */
    VmMap::~VmMap()
    {
    }

    /**
     *
     */
    void
    VmMap::_init(void)
    {
      usedareas.next = &usedareas;
      usedareas.prev = &usedareas;
      usedareas.p_addr = 1;
      usedareas.v_addr = 1;
      usedareas.length = 0;

      freeareas.next = &freeareas;
      freeareas.prev = &freeareas;
      freeareas.p_addr = 0xb0000000;
      freeareas.v_addr = 0xb0000000;
      freeareas.length = 0x10000000;
    }
  } // mem
} // kernel
